#ifndef _BASIC_
#define _BASIC_

#include <opencv2/opencv.hpp>
#include "complement.h"
#include <Eigen/Dense>
#include "Convert.hpp"
#include "Feature.hpp"
#include <sophus/se3.h>
#include <ceres/rotation.h>

typedef Eigen::Matrix<double,3,3,Eigen::RowMajor> Matrix3d;



struct SingleScaleStructrue
{
    cv::Mat img;
	std::vector<KeyPoint> kpts;
	std::vector<Point3f> p3d;
    cv::Mat desp;
    std::vector<Feature*> fts;
    SingleScaleStructrue(){};
    SingleScaleStructrue(const SingleScaleStructrue& a);
    ~SingleScaleStructrue()
    {
        for(auto p:fts)
            delete p;
        img.~Mat();
        desp.~Mat();
        kpts.clear();
        p3d.clear();
        fts.clear();
    }
};




struct MultiScaleStructure
{
    std::vector<Mat> allImgs;
    std::vector<vector<KeyPoint>> allKpts;
    std::vector<vector<Point3f>> allP3Ds;
    std::vector<Mat> allDesp;
    std::vector<vector<Feature*>> allFts;
    float scale;
    int levelNum;
    MultiScaleStructure(const cv::Mat &img, float scale, int levelNum);
    MultiScaleStructure();
    MultiScaleStructure(const MultiScaleStructure&a);
    ~MultiScaleStructure()
    {
        for(auto &a:allFts)
            for(auto b:a)
                delete b;
    }
};


class Pose
{
public:
    Pose(const cv::Mat &_R, const cv::Mat &_t) :R_(_R), t_(_t) {};
    Pose(const Matrix3d &_R,const Eigen::Vector3d _t):R(_R),t(_t){};
	Pose(){};
	Pose(const Pose&b)=default;
	void applyTranform(const double *p);
    Eigen::Vector4d toQuaternion()const;
    Eigen::Vector3d EulerAngle()const;
    Eigen::Vector3d AngleAxis()const
    {
        return Eigen::AngleAxisd(R).axis()*Eigen::AngleAxisd(R).angle();
    }
    cv::Matx33d R_;
    cv::Matx31d t_;
    Matrix3d R;
    Eigen::Vector3d t;
    inline Eigen::Vector3d map(const Eigen::Vector3d&a)const
    {
        return R*a+t;
    }
    Eigen::Vector3d map(const Eigen::Vector3d&,Eigen::Matrix<double,3,6>&J)const;
    Eigen::Matrix<double,6,1> se3();
    cv::Scalar q;
	template<typename T>
  void TransformToCameraPlane(const cv::Point3_<T> &s, cv::Point3_<T>&d);
	template<typename T>
  void TransformToCameraPlane(const std::vector<cv::Point3_<T>> &src, std::vector<cv::Point3_<T>> &dst);
	template<typename T>
  void TransformToCameraPlane(const cv::Matx<T, 3, 1> &s, cv::Matx<T, 2, 1>&d);
	template<typename T>
  void TransformToCameraPlane(const std::vector<cv::Matx<T, 3, 1>> &src, std::vector<cv::Matx<T, 2, 1>> &dst);
private:
    void toEulerAngle(double p[3])const;
    
};

class IntrinsicMatrix
{
public:
    IntrinsicMatrix(const Eigen::Matrix3d&_K) :K(_K), fx(K(0, 0)), fy(K(1, 1)), cx(K(0, 2)), cy(K(1, 2)) {K_=Converter::Eigen2CV(K);};
	~IntrinsicMatrix() {};
	IntrinsicMatrix(const IntrinsicMatrix &b);
	IntrinsicMatrix applyScale(double scale)const;
    Eigen::Vector2d project(const Eigen::Vector3d& a,Eigen::Matrix<double,2,3>&J)const;
    Eigen::Vector2d project(const Eigen::Vector3d& a)const;
    Eigen::Vector3d cam2world(const Eigen::Vector2d&a)const;
    Eigen::Matrix3d K;
    cv::Matx33d K_;
	double fx, fy, cx, cy;
	template<typename T, typename T2>
	void projectToImage(const vector<Point3_<T>> &s, vector<Point_<T2>> &dst);
	template<typename T, typename T2>
	void projectToImage(const Point3_<T> &s, Point_<T2> &d);
private:

};


template<typename T, typename T2>
inline void IntrinsicMatrix::projectToImage(const vector<Point3_<T>> &s, vector<Point_<T2>> &d)
{
	d.resize(s.size());
	for (size_t i = 0; i < s.size(); i++)
	{
		projectToImage(s[i], d[i]);
	}
}

template<typename T, typename T2>
inline void IntrinsicMatrix::projectToImage(const Point3_<T> &s, Point_<T2> &d)
{
	d.x = s.x * fx / s.z + cx;
	d.y = s.y * fy / s.z + cy;
}

template<typename T>
inline void Pose::TransformToCameraPlane(const Point3_<T> &s, Point3_<T>&d)
{
	d.x = s.x*R(0, 0) + s.y*R(1, 0) + s.z*R(2, 0) + t(0);
	d.y = s.x*R(0, 1) + s.y*R(1, 1) + s.z*R(2, 1) + t(1);
	d.z = s.x*R(0, 2) + s.y*R(1, 2) + s.z*R(2, 2) + t(2);


	//    const auto &r = (const double*)R.data;
	//    const auto &tt = (const double*)t.data;
	//    d.x = s.x*r[0] + s.y*r[1] + s.z*r[2] + tt[0];
	//    d.y = s.x*r[3] + s.y*r[4] + s.z*r[5] + tt[1];
	//    d.z = s.x*r[6] + s.y*r[7] + s.z*r[8] + tt[2];
}

template<typename T>
inline void Pose::TransformToCameraPlane(const vector<Point3_<T>> &src, vector<Point3_<T>> &dst)
{
	//	CV_Assert(R.type() == CV_64F&&t.type() == CV_64F);
	dst.clear();
	dst.resize(src.size());

	for (size_t i = 0; i < src.size(); i++)
	{
		const auto &s = src[i];
		auto &d = dst[i];
		TransformToCameraPlane(s, d);
	}
}

template<typename T>
inline void Pose::TransformToCameraPlane(const Matx<T, 3, 1>& s, Matx<T, 2, 1>& d)
{
	d(0) = s(1)*R(0, 0) + s(1)*R(1, 0) + s(2)*R(2, 0) + t(0);
	d(1) = s(0)*R(0, 1) + s(1)*R(1, 1) + s(2)*R(2, 1) + t(1);
	d(2) = s(0)*R(0, 2) + s(1)*R(1, 2) + s(2)*R(2, 2) + t(2);
}

template<typename T>
inline void TransformToCameraPlane(const vector<Matx<T, 3, 1>> &src, vector<Matx<T, 2, 1>> &dst)
{
	dst.clear();
	dst.resize(src.size());

	for (size_t i = 0; i < src.size(); i++)
	{
		const auto &s = src[i];
		auto &d = dst[i];
		TransformToCameraPlane(s, d);
	}
}



#endif